<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<title>GSoC2011SfM: D:/Travail/These/Determination caracteristiques camera/GSoC/SfM/src/PointOfView.cpp Source File</title>

<link href="tabs.css" rel="stylesheet" type="text/css"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />

<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>

</head>
<body>
<div id="top"><!-- do not remove this div! -->


<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  
  <td id="projectlogo"><img alt="Logo" src="logo.png"/></td>
  
  
  <td style="padding-left: 0.5em;">
   <div id="projectname">GSoC2011SfM
   &#160;<span id="projectnumber">0.1</span>
   </div>
   <div id="projectbrief">Google Summer of Code 2011: Structure from motion</div>
  </td>
  
  
  
 </tr>
 </tbody>
</table>
</div>

<!-- Generated by Doxygen 1.7.5.1 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
    </ul>
  </div>
<div class="header">
  <div class="headertitle">
<div class="title">D:/Travail/These/Determination caracteristiques camera/GSoC/SfM/src/PointOfView.cpp</div>  </div>
</div>
<div class="contents">
<div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00002"></a>00002 <span class="preprocessor">#include &quot;macro.h&quot;</span> <span class="comment">//SFM_EXPORTS &amp; remove annoying warnings</span>
<a name="l00003"></a>00003 
<a name="l00004"></a>00004 <span class="preprocessor">#include &quot;opencv2/highgui/highgui.hpp&quot;</span>
<a name="l00005"></a>00005 <span class="preprocessor">#include &quot;pcl/visualization/pcl_visualizer.h&quot;</span>
<a name="l00006"></a>00006 <span class="preprocessor">#include &lt;iostream&gt;</span>
<a name="l00007"></a>00007 <span class="preprocessor">#include &lt;pcl/io/vtk_io.h&gt;</span>
<a name="l00008"></a>00008 <span class="preprocessor">#include &quot;opencv2/core/eigen.hpp&quot;</span>
<a name="l00009"></a>00009 <span class="preprocessor">#include &quot;libmv/multiview/projection.h&quot;</span>
<a name="l00010"></a>00010 
<a name="l00011"></a>00011 <span class="preprocessor">#include &quot;PointOfView.h&quot;</span>
<a name="l00012"></a>00012 
<a name="l00013"></a>00013 <span class="preprocessor">#include &quot;TracksOfPoints.h&quot;</span>
<a name="l00014"></a>00014 <span class="preprocessor">#include &quot;PCL_mapping.h&quot;</span>
<a name="l00015"></a>00015 <span class="preprocessor">#include &quot;CameraPinholeDistor.h&quot;</span>
<a name="l00016"></a>00016 
<a name="l00017"></a>00017 <span class="keyword">using</span> cv::Mat;
<a name="l00018"></a>00018 <span class="keyword">using</span> cv::Vec4d;
<a name="l00019"></a>00019 <span class="keyword">using</span> cv::Vec3d;
<a name="l00020"></a>00020 <span class="keyword">using</span> cv::Vec2d;
<a name="l00021"></a>00021 <span class="keyword">using</span> cv::Vec;
<a name="l00022"></a>00022 <span class="keyword">using</span> cv::Range;
<a name="l00023"></a>00023 <span class="keyword">using</span> cv::Ptr;
<a name="l00024"></a>00024 <span class="keyword">using</span> std::vector;
<a name="l00025"></a>00025 <span class="keyword">using</span> std::string;
<a name="l00026"></a>00026 <span class="keyword">using</span> cv::imread;
<a name="l00027"></a>00027 <span class="keyword">using</span> libmv::Mat34;
<a name="l00028"></a>00028 <span class="keyword">using</span> libmv::Mat3;
<a name="l00029"></a>00029 <span class="keyword">using</span> libmv::Vec3;
<a name="l00030"></a>00030 <span class="keyword">using</span> libmv::Vec;
<a name="l00031"></a>00031 
<a name="l00032"></a>00032 <span class="keyword">namespace </span>OpencvSfM{
<a name="l00033"></a>00033 
<a name="l00034"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a296c28b5a2c8412a62ae3a7752a2c850">00034</a>   <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a296c28b5a2c8412a62ae3a7752a2c850">PointOfView::PointOfView</a>( cv::Ptr&lt;Camera&gt; device, 
<a name="l00035"></a>00035     cv::Mat rotation <span class="comment">/*=Mat::eye( 3, 3, CV_64F )*/</span>, 
<a name="l00036"></a>00036     cv::Vec3d translation <span class="comment">/*=Vec( 0.0,0.0,0.0 )*/</span> )
<a name="l00037"></a>00037     : projection_matrix_( 3, 4, CV_64F )
<a name="l00038"></a>00038   {
<a name="l00039"></a>00039     CV_DbgAssert( rotation.rows==3 &amp;&amp; rotation.cols==3 );
<a name="l00040"></a>00040     CV_DbgAssert( !device.empty( ) );
<a name="l00041"></a>00041 
<a name="l00042"></a>00042     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>=device;
<a name="l00043"></a>00043 
<a name="l00044"></a>00044     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a4593900dcb330a47fdb5a7a1c4e2ad93" title="Rotation matrix R ( data is stored into projection_matrix_ )">rotation_</a>=<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a>( Range::all( ),Range( 0,3 ));
<a name="l00045"></a>00045     rotation.copyTo( this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a4593900dcb330a47fdb5a7a1c4e2ad93" title="Rotation matrix R ( data is stored into projection_matrix_ )">rotation_</a> );
<a name="l00046"></a>00046 
<a name="l00047"></a>00047     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#acd8ff3683942c1a8afed7b4f266eaa20" title="Translation vector t ( Matrix instead of vector because data is stored into projection_matrix_ )...">translation_</a> = <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a>( Range::all( ),Range( 3,4 ));
<a name="l00048"></a>00048     Mat( translation ).copyTo( this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#acd8ff3683942c1a8afed7b4f266eaa20" title="Translation vector t ( Matrix instead of vector because data is stored into projection_matrix_ )...">translation_</a> );
<a name="l00049"></a>00049 
<a name="l00050"></a>00050     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#ac644fc5f92709b33a31ee2370365f104" title="This attribut is used to know what we should estimate... If equal to 0, nothing should be estimated...">config_</a>=0;<span class="comment">//everything should be estimated...</span>
<a name="l00051"></a>00051 
<a name="l00052"></a>00052     <span class="comment">//as we are a new point of view related to a device, we should add our address into device_:</span>
<a name="l00053"></a>00053     device_-&gt;pointsOfView_.push_back( <span class="keyword">this</span> );
<a name="l00054"></a>00054   };
<a name="l00055"></a>00055 
<a name="l00056"></a>00056 
<a name="l00057"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a4f84ebb671f2a76c94203120cca7d111">00057</a>   <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a296c28b5a2c8412a62ae3a7752a2c850">PointOfView::PointOfView</a>( cv::Mat projection_matrix )
<a name="l00058"></a>00058     : projection_matrix_( 3, 4, CV_64F )
<a name="l00059"></a>00059   {
<a name="l00060"></a>00060     <span class="comment">//convert to libmv matrix:</span>
<a name="l00061"></a>00061     libmv::Mat34 proj;
<a name="l00062"></a>00062     cv::cv2eigen( projection_matrix, proj );
<a name="l00063"></a>00063     libmv::Mat3 K, R;
<a name="l00064"></a>00064     libmv::Vec3 t;
<a name="l00065"></a>00065     libmv::KRt_From_P( proj, &amp;K, &amp;R, &amp;t );
<a name="l00066"></a>00066     
<a name="l00067"></a>00067     <span class="keywordflow">if</span> (R.determinant() &lt; 0) {
<a name="l00068"></a>00068       K.col(0) = -K.col(0);
<a name="l00069"></a>00069       R.row(0) = -R.row(0);
<a name="l00070"></a>00070       t(0)     = -t(0);
<a name="l00071"></a>00071     }
<a name="l00072"></a>00072     
<a name="l00073"></a>00073     <span class="comment">//enforce the rotation matrix.</span>
<a name="l00074"></a>00074     <span class="comment">//TODO find the closest rotation matrix...</span>
<a name="l00075"></a>00075     Eigen::Quaterniond tmp = (Eigen::Quaterniond)R;
<a name="l00076"></a>00076     tmp.normalize();
<a name="l00077"></a>00077     R = tmp.toRotationMatrix();
<a name="l00078"></a>00078 
<a name="l00079"></a>00079     <span class="comment">//create the device:</span>
<a name="l00080"></a>00080     cv::Mat K_cv;
<a name="l00081"></a>00081     cv::eigen2cv( K, K_cv );
<a name="l00082"></a>00082     <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a> = cv::Ptr&lt;Camera&gt;( <span class="keyword">new</span> <a class="code" href="class_opencv_sf_m_1_1_camera_pinhole.html" title="This class represent the physical device which take the pictures. It is not related to a 3D position ...">CameraPinhole</a>( K_cv ) );
<a name="l00083"></a>00083 
<a name="l00084"></a>00084     proj&lt;&lt;R,t;
<a name="l00085"></a>00085     cv::eigen2cv( proj, <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a> );
<a name="l00086"></a>00086 
<a name="l00087"></a>00087     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a4593900dcb330a47fdb5a7a1c4e2ad93" title="Rotation matrix R ( data is stored into projection_matrix_ )">rotation_</a>=<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a>( Range::all( ),Range( 0,3 ));
<a name="l00088"></a>00088 
<a name="l00089"></a>00089     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#acd8ff3683942c1a8afed7b4f266eaa20" title="Translation vector t ( Matrix instead of vector because data is stored into projection_matrix_ )...">translation_</a> = <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a>( Range::all( ),Range( 3,4 ));
<a name="l00090"></a>00090 
<a name="l00091"></a>00091     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#ac644fc5f92709b33a31ee2370365f104" title="This attribut is used to know what we should estimate... If equal to 0, nothing should be estimated...">config_</a>=0;<span class="comment">//everything should be estimated...</span>
<a name="l00092"></a>00092 
<a name="l00093"></a>00093     <span class="comment">//as we are a new point of view related to a device, we should add our address into device_:</span>
<a name="l00094"></a>00094     <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;pointsOfView_.push_back( <span class="keyword">this</span> );
<a name="l00095"></a>00095 
<a name="l00096"></a>00096   };
<a name="l00097"></a>00097 
<a name="l00098"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#aff68fe7602ebc6effa817860cea983f7">00098</a>   <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#aff68fe7602ebc6effa817860cea983f7">PointOfView::~PointOfView</a>( <span class="keywordtype">void</span> )
<a name="l00099"></a>00099   {
<a name="l00100"></a>00100     this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a>.release( );
<a name="l00101"></a>00101 
<a name="l00102"></a>00102     <span class="comment">//remove the reference in device_-&gt;pointsOfView_:</span>
<a name="l00103"></a>00103     vector&lt;PointOfView*&gt;::iterator ourRef=<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;pointsOfView_.begin( );
<a name="l00104"></a>00104     <span class="keywordtype">bool</span> isFound=<span class="keyword">false</span>;
<a name="l00105"></a>00105     <span class="keywordflow">while</span>( !isFound &amp;&amp; ourRef != <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;pointsOfView_.end( ) )
<a name="l00106"></a>00106     {
<a name="l00107"></a>00107       <span class="comment">//be aware of NULL pointor:</span>
<a name="l00108"></a>00108       <span class="keywordflow">if</span>( ( *ourRef ) == this )
<a name="l00109"></a>00109       {
<a name="l00110"></a>00110         isFound=<span class="keyword">true</span>;
<a name="l00111"></a>00111         ( *ourRef )=NULL;
<a name="l00112"></a>00112       }
<a name="l00113"></a>00113       ourRef++;
<a name="l00114"></a>00114     }
<a name="l00115"></a>00115   }
<a name="l00116"></a>00116 
<a name="l00117"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#aefde879830522099935e19c92968f3f0">00117</a>   cv::Vec2d <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#aefde879830522099935e19c92968f3f0">PointOfView::project3DPointIntoImage</a>( cv::Vec3d point )<span class="keyword"> const</span>
<a name="l00118"></a>00118 <span class="keyword">  </span>{
<a name="l00119"></a>00119     <span class="comment">//As we don&#39;t know what type of camera we use ( with/without disportion, fisheyes... )</span>
<a name="l00120"></a>00120     <span class="comment">//we can&#39;t use classic projection matrix P = K . [ R|t ]</span>
<a name="l00121"></a>00121     <span class="comment">//Instead, we first compute points transformation into camera&#39;s system and then compute</span>
<a name="l00122"></a>00122     <span class="comment">//pixel coordinate using camera device function.</span>
<a name="l00123"></a>00123 
<a name="l00124"></a>00124     <span class="comment">//As we need Mat object to compute projection, we create temporary objects:</span>
<a name="l00125"></a>00125     Mat mat3DNorm( 4,1,CV_64F );
<a name="l00126"></a>00126     <span class="keywordtype">double</span>* point3DNorm=( <span class="keywordtype">double</span>* )mat3DNorm.data;
<a name="l00127"></a>00127     Mat mat2DNorm( 3,1,CV_64F );
<a name="l00128"></a>00128     <span class="keywordtype">double</span>* point2DNorm=( <span class="keywordtype">double</span>* )mat2DNorm.data;
<a name="l00129"></a>00129 
<a name="l00130"></a>00130     vector&lt;Vec2d&gt; pointsOut;
<a name="l00131"></a>00131     point3DNorm[ 0 ] = point[ 0 ];
<a name="l00132"></a>00132     point3DNorm[ 1 ] = point[ 1 ];
<a name="l00133"></a>00133     point3DNorm[ 2 ] = point[ 2 ];
<a name="l00134"></a>00134     point3DNorm[ 3 ] = 1;
<a name="l00135"></a>00135 
<a name="l00136"></a>00136     <span class="comment">//transform points into camera&#39;s coordinates:</span>
<a name="l00137"></a>00137     mat2DNorm = ( <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a> * mat3DNorm );
<a name="l00138"></a>00138     pointsOut.push_back( Vec2d( point2DNorm[ 0 ]/point2DNorm[ 2 ],point2DNorm[ 1 ]/point2DNorm[ 2 ] ));
<a name="l00139"></a>00139 
<a name="l00140"></a>00140     <span class="comment">//transform points into pixel coordinates using camera intra parameters:</span>
<a name="l00141"></a>00141     pointsOut = <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;normImageToPixelCoordinates( pointsOut );
<a name="l00142"></a>00142     Vec2d pointOut = pointsOut[ 0 ];
<a name="l00143"></a>00143     <span class="keywordflow">return</span> pointsOut[ 0 ];
<a name="l00144"></a>00144   }
<a name="l00145"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#ae83f148293bc3cb6cb3e52a5bf89d6f3">00145</a>   std::vector&lt; cv::Vec2d &gt; <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#aea728bb630de5161ce54f6b5639adbd7">PointOfView::project3DPointsIntoImage</a>( 
<a name="l00146"></a>00146     std::vector&lt; cv::Vec3d &gt; points )<span class="keyword"> const</span>
<a name="l00147"></a>00147 <span class="keyword">  </span>{
<a name="l00148"></a>00148     <span class="comment">//As we don&#39;t know what type of camera we use ( with/without disportion, fisheyes... )</span>
<a name="l00149"></a>00149     <span class="comment">//we can&#39;t use classic projection matrix P = K . [ R|t ]</span>
<a name="l00150"></a>00150     <span class="comment">//Instead, we first compute points transformation into camera&#39;s system and then compute</span>
<a name="l00151"></a>00151     <span class="comment">//pixel coordinate using camera device function.</span>
<a name="l00152"></a>00152 
<a name="l00153"></a>00153     <span class="comment">//As we need Mat object to compute projection, we create temporary objects:</span>
<a name="l00154"></a>00154     Mat mat3DNorm( 4,1,CV_64F );
<a name="l00155"></a>00155     <span class="keywordtype">double</span>* point3DNorm=( <span class="keywordtype">double</span>* )mat3DNorm.data;
<a name="l00156"></a>00156     Mat mat2DNorm( 3,1,CV_64F );
<a name="l00157"></a>00157     <span class="keywordtype">double</span>* point2DNorm=( <span class="keywordtype">double</span>* )mat2DNorm.data;
<a name="l00158"></a>00158     
<a name="l00159"></a>00159     vector&lt;Vec2d&gt; pointsOut;
<a name="l00160"></a>00160     vector&lt;Vec3d&gt;::iterator point=points.begin( );
<a name="l00161"></a>00161     <span class="keywordflow">while</span>( point!=points.end( ) )
<a name="l00162"></a>00162     {
<a name="l00163"></a>00163       point3DNorm[ 0 ] = ( *point )[ 0 ];
<a name="l00164"></a>00164       point3DNorm[ 1 ] = ( *point )[ 1 ];
<a name="l00165"></a>00165       point3DNorm[ 2 ] = ( *point )[ 2 ];
<a name="l00166"></a>00166       point3DNorm[ 3 ] = 1;
<a name="l00167"></a>00167 
<a name="l00168"></a>00168       <span class="comment">//transform points into camera&#39;s coordinates:</span>
<a name="l00169"></a>00169       mat2DNorm = ( <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a> * mat3DNorm );
<a name="l00170"></a>00170 
<a name="l00171"></a>00171       pointsOut.push_back( Vec2d( point2DNorm[ 0 ]/point2DNorm[ 2 ],point2DNorm[ 1 ]/point2DNorm[ 2 ] ));
<a name="l00172"></a>00172 
<a name="l00173"></a>00173       point++;
<a name="l00174"></a>00174     }
<a name="l00175"></a>00175     <span class="comment">//transform points into pixel coordinates using camera intra parameters:</span>
<a name="l00176"></a>00176     <span class="keywordflow">return</span> <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;normImageToPixelCoordinates( pointsOut );
<a name="l00177"></a>00177   }
<a name="l00178"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#aea728bb630de5161ce54f6b5639adbd7">00178</a>   std::vector&lt; cv::Vec2d &gt; <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#aea728bb630de5161ce54f6b5639adbd7">PointOfView::project3DPointsIntoImage</a>(
<a name="l00179"></a>00179     std::vector&lt;TrackOfPoints&gt; points )<span class="keyword"> const</span>
<a name="l00180"></a>00180 <span class="keyword">  </span>{
<a name="l00181"></a>00181     <span class="comment">//As we don&#39;t know what type of camera we use ( with/without disportion, fisheyes... )</span>
<a name="l00182"></a>00182     <span class="comment">//we can&#39;t use classic projection matrix P = K . [ R|t ]</span>
<a name="l00183"></a>00183     <span class="comment">//Instead, we first compute points transformation into camera&#39;s system and then compute</span>
<a name="l00184"></a>00184     <span class="comment">//pixel coordinate using camera device function.</span>
<a name="l00185"></a>00185 
<a name="l00186"></a>00186     <span class="comment">//As we need Mat object to compute projection, we create temporary objects:</span>
<a name="l00187"></a>00187     Mat mat3DNorm( 4,1,CV_64F );
<a name="l00188"></a>00188     <span class="keywordtype">double</span>* point3DNorm=( <span class="keywordtype">double</span>* )mat3DNorm.data;
<a name="l00189"></a>00189     Mat mat2DNorm( 3,1,CV_64F );
<a name="l00190"></a>00190     <span class="keywordtype">double</span>* point2DNorm=( <span class="keywordtype">double</span>* )mat2DNorm.data;
<a name="l00191"></a>00191     
<a name="l00192"></a>00192     vector&lt;Vec2d&gt; pointsOut;
<a name="l00193"></a>00193     vector&lt;TrackOfPoints&gt;::iterator point=points.begin( );
<a name="l00194"></a>00194     <span class="keywordflow">while</span>( point!=points.end( ) )
<a name="l00195"></a>00195     {
<a name="l00196"></a>00196       cv::Ptr&lt;Vec3d&gt; convert_from_track = point-&gt;get3DPosition();
<a name="l00197"></a>00197       <span class="keywordflow">if</span>( !convert_from_track.empty() )
<a name="l00198"></a>00198       {
<a name="l00199"></a>00199         point3DNorm[ 0 ] = (*convert_from_track)[ 0 ];
<a name="l00200"></a>00200         point3DNorm[ 1 ] = (*convert_from_track)[ 1 ];
<a name="l00201"></a>00201         point3DNorm[ 2 ] = (*convert_from_track)[ 2 ];
<a name="l00202"></a>00202         point3DNorm[ 3 ] = 1;
<a name="l00203"></a>00203 
<a name="l00204"></a>00204         <span class="comment">//transform points into camera&#39;s coordinates:</span>
<a name="l00205"></a>00205         mat2DNorm = ( <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a> * mat3DNorm );
<a name="l00206"></a>00206 
<a name="l00207"></a>00207         pointsOut.push_back( Vec2d( point2DNorm[ 0 ]/point2DNorm[ 2 ],point2DNorm[ 1 ]/point2DNorm[ 2 ] ));
<a name="l00208"></a>00208       }
<a name="l00209"></a>00209 
<a name="l00210"></a>00210       point++;
<a name="l00211"></a>00211     }
<a name="l00212"></a>00212     <span class="comment">//transform points into pixel coordinates using camera intra parameters:</span>
<a name="l00213"></a>00213     <span class="keywordflow">return</span> <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;normImageToPixelCoordinates( pointsOut );
<a name="l00214"></a>00214   }
<a name="l00215"></a>00215 
<a name="l00216"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a88697becf69e7291d7c08dc3eb5e891e">00216</a>   <span class="keywordtype">bool</span> <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a88697becf69e7291d7c08dc3eb5e891e">PointOfView::pointInFrontOfCamera</a>( cv::Vec4d point )<span class="keyword"> const</span>
<a name="l00217"></a>00217 <span class="keyword">  </span>{
<a name="l00218"></a>00218     Mat pointTranspose= ( Mat ) point;
<a name="l00219"></a>00219     <span class="keywordtype">double</span> condition_1 = 
<a name="l00220"></a>00220       this-&gt;<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a>.row( 2 ).dot( pointTranspose.t( ) ) * point[ 3 ];
<a name="l00221"></a>00221     <span class="keywordtype">double</span> condition_2 = point[ 2 ] * point[ 3 ];
<a name="l00222"></a>00222     <span class="keywordflow">if</span>( condition_1 &gt; 0 &amp;&amp; condition_2 &gt; 0 )
<a name="l00223"></a>00223       <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00224"></a>00224     <span class="keywordflow">else</span>
<a name="l00225"></a>00225       <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00226"></a>00226   }
<a name="l00227"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a26848a2211358fb86b69fda1376c78c7">00227</a>   cv::Mat <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a26848a2211358fb86b69fda1376c78c7">PointOfView::getProjectionMatrix</a>( )<span class="keyword"> const</span>
<a name="l00228"></a>00228 <span class="keyword">  </span>{
<a name="l00229"></a>00229     <span class="keywordflow">return</span> <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;getIntraMatrix( ) * <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a920f9386c300582a19e68908654d169c" title="redundancy but speed improvement">projection_matrix_</a>;
<a name="l00230"></a>00230   };
<a name="l00231"></a>00231 
<a name="l00232"></a>00232 
<a name="l00233"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a5f642f693bbdb642baf1bae2ae63c85e">00233</a>   cv::Ptr&lt;PointOfView&gt; <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a5f642f693bbdb642baf1bae2ae63c85e">PointOfView::read</a>( <span class="keyword">const</span> cv::FileNode&amp; node )
<a name="l00234"></a>00234   {
<a name="l00235"></a>00235     std::string myName=node.name( );
<a name="l00236"></a>00236     <span class="keywordflow">if</span>( myName != <span class="stringliteral">&quot;PointOfView&quot;</span> )
<a name="l00237"></a>00237     {
<a name="l00238"></a>00238       std::string error = <span class="stringliteral">&quot;FileNode is not correct!\nExpected \&quot;PointOfView\&quot;, got &quot;</span>;
<a name="l00239"></a>00239       error += node.name();
<a name="l00240"></a>00240       CV_Error( CV_StsError, error.c_str() );
<a name="l00241"></a>00241     }
<a name="l00242"></a>00242     cv::FileNodeIterator it = node.begin();
<a name="l00243"></a>00243     cv::Mat rot,trans;
<a name="l00244"></a>00244     cv::Ptr&lt;Camera&gt; device;
<a name="l00245"></a>00245 
<a name="l00246"></a>00246     <span class="keywordflow">while</span>( it != node.end() )
<a name="l00247"></a>00247     {
<a name="l00248"></a>00248       <span class="keywordflow">if</span>((*it).name()==<span class="stringliteral">&quot;rotation_&quot;</span>)
<a name="l00249"></a>00249         (*it)&gt;&gt;rot;
<a name="l00250"></a>00250       <span class="keywordflow">if</span>((*it).name()==<span class="stringliteral">&quot;translation_&quot;</span>)
<a name="l00251"></a>00251         (*it)&gt;&gt;trans;
<a name="l00252"></a>00252       <span class="keywordflow">if</span>((*it).name()==<span class="stringliteral">&quot;CameraPinhole&quot;</span>)
<a name="l00253"></a>00253         device = <a class="code" href="class_opencv_sf_m_1_1_camera_pinhole.html#a799aa0db013e8d1e8516157a4905c36b">CameraPinhole::read</a>( *it );
<a name="l00254"></a>00254       <span class="keywordflow">if</span>((*it).name()==<span class="stringliteral">&quot;CameraPinholeDistor&quot;</span>)
<a name="l00255"></a>00255         device = <a class="code" href="class_opencv_sf_m_1_1_camera_pinhole_distor.html#a449b7e76f4e82b50b38f01457fdfa77b">CameraPinholeDistor::read</a>( *it );
<a name="l00256"></a>00256       it++;
<a name="l00257"></a>00257     }
<a name="l00258"></a>00258     <span class="keywordflow">return</span> cv::Ptr&lt;PointOfView&gt;( <span class="keyword">new</span> <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a296c28b5a2c8412a62ae3a7752a2c850">PointOfView</a>(device, rot, trans) );
<a name="l00259"></a>00259   }
<a name="l00260"></a>00260 
<a name="l00261"></a><a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#ab1b105f23162533972b1a88e3b8b0982">00261</a>   <span class="keywordtype">void</span> <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#ab1b105f23162533972b1a88e3b8b0982">PointOfView::write</a>( cv::FileStorage&amp; fs, <span class="keyword">const</span> <a class="code" href="class_opencv_sf_m_1_1_point_of_view.html" title="This class represent the 3D position of the device which take the pictures. The role of the class is ...">PointOfView</a>&amp; pov )
<a name="l00262"></a>00262   {
<a name="l00263"></a>00263     fs &lt;&lt;<span class="stringliteral">&quot;{&quot;</span> &lt;&lt; <span class="stringliteral">&quot;PointOfView&quot;</span> &lt;&lt;<span class="stringliteral">&quot;{&quot;</span> &lt;&lt; <span class="stringliteral">&quot;rotation_&quot;</span> &lt;&lt; pov.<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a4593900dcb330a47fdb5a7a1c4e2ad93" title="Rotation matrix R ( data is stored into projection_matrix_ )">rotation_</a>;
<a name="l00264"></a>00264     fs &lt;&lt; <span class="stringliteral">&quot;translation_&quot;</span> &lt;&lt; pov.<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#acd8ff3683942c1a8afed7b4f266eaa20" title="Translation vector t ( Matrix instead of vector because data is stored into projection_matrix_ )...">translation_</a>;
<a name="l00265"></a>00265     pov.<a class="code" href="class_opencv_sf_m_1_1_point_of_view.html#a83bfd58fd1074d5950dad2be9630dbf8" title="intra parameters and distortion coefs">device_</a>-&gt;write( fs );
<a name="l00266"></a>00266     fs &lt;&lt; <span class="stringliteral">&quot;}&quot;</span> &lt;&lt; <span class="stringliteral">&quot;}&quot;</span>;
<a name="l00267"></a>00267   }
<a name="l00268"></a>00268 
<a name="l00269"></a>00269 }
</pre></div></div>
</div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>Variables</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>



<hr class="footer"/><address class="footer"><small>
Generated on Sun Aug 21 2011 16:45:52 for GSoC2011SfM by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.7.5.1
</small></address>

</body>
</html>
